#!/usr/bin/env python

import roslib; roslib.load_manifest('brain_stuff')
import rospy
import image_geometry
import message_filters
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
import cv
import math
import copy
import sys
import array
from PySide.QtCore import *
from PySide.QtGui import *
import smach
from std_msgs.msg import Float32

# Convert cv image to qimage...
class IplQImage(QImage):
   def __init__(self, iplimage):
      rgba = cv.CreateMat(iplimage.height, iplimage.width, cv.CV_8UC4)
      cv.Set(rgba, (1,2,3,4))

      cv.MixChannels([iplimage], [rgba], [ (0,0), (1,1), (2,2), (3,3) ] ) # This encoding works because I explicitly convert the imgmsg to cv in rgba
      self.__imagedata = rgba.tostring()
      super(IplQImage,self).__init__(self.__imagedata, iplimage.width, iplimage.height, QImage.Format_RGB32)


class Interface_Pyside(smach.State):
   def __init__(self):
      # Smach
      smach.State.__init__(self, outcomes=['success', 'aborted'])
      # Flags and global vars
      self.cv_image = None
      # OpenCv setup
      self.bridge = CvBridge()
      # Publishers and Subscribers
      self.image_sub = rospy.Subscriber("/wide_stereo/right/image_rect_color", Image, self.callback)
      # Pyside
      self.group = QGroupBox('group1')
      self.label2 = QLabel()
      self.label2.setText('Hello World')
      self.i = 0
      vbox = QVBoxLayout()
      vbox.addWidget(self.label2)
      self.group.setLayout(vbox)

   def execute(self, userdata):
      try:
         self.group.show()
         app.exec_()
      except KeyboardInterrupt:
         app.quit()
         return 'aborted'
      app.quit()
      return 'success' 

   def showImage(self):
      if self.cv_image == None:
         rospy.loginfo("Trying to display an image before it exists! Skipping.")
         return
      rospy.loginfo("showImage")

      # If the text is used then this works.
      #label2 = QLabel()
      self.label2.setText('Hello World {0}'.format(self.i))
      self.i = self.i+1

      # Same thing with pixmaps doesn't work
      #qimage = IplQImage(self.cv_image)
      #qpixmap = QPixmap.fromImage(qimage)
      #self.label2.setPixmap(qpixmap)

      
   def callback(self, data):
      rospy.loginfo("Received image")
      if rospy.is_shutdown():
         return
      try:
         self.cv_image = self.bridge.imgmsg_to_cv(data, "rgba8")
      except CvBridgeError, e:
         print e
         return
      self.showImage()

class Interface(smach.State):
   def __init__(self):
      smach.State.__init__(self, outcomes=['success', 'aborted'])
      # Flags and global vars
      self.cv_image = None
      self.cam_model = None
      self.start = None
      self.end = None
      self.outlining = False
      self.done = False

      # OpenCv setup
      self.window_name = "Image Window"
      cv.NamedWindow(self.window_name)
      self.bridge = CvBridge()
      cv.SetMouseCallback(self.window_name, self.mouse_callback)

      # Publishers and Subscriber
      self.image_sub2 = rospy.Subscriber("/wide_stereo/right/image_rect_color", Image, self.callback)
      #self.image_sub = message_filters.Subscriber("/wide_stereo/right/image_rect", Image)
      #self.camera_info_sub = message_filters.Subscriber("/wide_stereo/right/camera_info", CameraInfo)
      #self.sync = message_filters.TimeSynchronizer([self.image_sub, self.camera_info_sub], 10)
      #self.sync.registerCallback(lambda x,y: self.callback(x, y))

   def execute(self, userdata):
      # Reset
      self.start = None
      self.end = None
      self.done = False
      self.outlining = False
      
      # Wait
      while((not self.done) and (not rospy.is_shutdown())):
         rospy.sleep(0.1)

      # Abort
      if rospy.is_shutdown():
         rospy.loginfo("Keyboard Interrupt received. Aborting")
         cv.DestroyAllWindows()
         return 'aborted'

      # Next
      self.done = True
      return 'success' 

         
   def showImage(self):
      if self.cv_image == None:
         rospy.logerror("Trying to display an image before it exists! Skipping.")
         return
      if self.start and self.end:
         cv.Rectangle(self.cv_image, self.start, self.end, (0,255,0), 1)
      cv.ShowImage(self.window_name, self.cv_image)
      cv.WaitKey(2) #Any less than 2 and it freezes when clicked... :(
      
   #def callback(self, data, cam_info):
   def callback(self, data):
      if rospy.is_shutdown():
         return
      #if self.cam_model == None:
         #self.cam_model = image_geometry.PinholeCameraModel()
         #self.cam_model.fromCameraInfo(cam_info)
      try:
         self.cv_image = self.bridge.imgmsg_to_cv(data, "rgb8")
      except CvBridgeError, e:
         print e
         return
      self.showImage()

   def mouse_callback(self, event, x, y, flags, param):
      if self.done:
         return
      if event == cv.CV_EVENT_LBUTTONDOWN:
         self.start = (x,y)
         self.end = (x,y)
         self.outlining = True
      elif event == cv.CV_EVENT_MOUSEMOVE and self.outlining:
         self.end = (x,y)
      elif event == cv.CV_EVENT_LBUTTONUP:
         self.outlining = False
         self.done = True
      self.showImage()

 
class TurnKnob(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded', 'failed'])

        self.layout = QVBoxLayout()
        self.dial = QDial()
        self.dial.valueChanged.connect(self.callback)
        self.dial.setNotchesVisible(True)
        self.dial.setWrapping(True)
        self.layout.addWidget(self.dial)

        self.button = QPushButton('done')
        self.button.clicked.connect(self.done)
        self.layout.addWidget(self.button)

        self.group = QGroupBox('knob control')
        self.group.setLayout(self.layout)

        self.pub = rospy.Publisher('knob/value', Float32)

    def callback(self, value):
        self.pub.publish(float(value) / 99.0 * 3.14159 * 2.0)

    def done(self):
        self.group.hide()
        app.quit()

    def execute(self, userdata):
        self.group.show()
        app.exec_()

        return 'succeeded'



if __name__== "__main__":
   rospy.init_node('brain_interface', log_level=rospy.DEBUG)

   #app = QApplication(sys.argv)
   machine = smach.StateMachine(outcomes = ['success', 'aborted'])
   with machine:
      smach.StateMachine.add("Interface", Interface_Pyside(), transitions={'aborted':'aborted', 'success':'Interface'})
      #smach.StateMachine.add("Interface", Interface(), transitions={'aborted':'aborted', 'success':'Turn Knob'})
      #smach.StateMachine.add("Turn Knob", TurnKnob(), transitions={'aborted':'aborted', 'success':'Interface'})
      #smach.StateMachine.add("Turn Knob", TurnKnob(), transitions={'succeeded':'success', 'failed':'Turn Knob'})
   try:
      machine.execute()
   except KeyboardInterrupt:
      exit()


